#include "DBoW3.h"
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <iostream>
#include <vector>
#include <string>

#include "BEBLID.h"
#include <chrono>

using namespace cv;
using namespace std;

/***************************************************
 * 本节演示了如何根据data/目录下的十张图训练字典
 * 对应<SLAM十四讲> P293代码
 *
 * 我们对10张目标图片提取的ORB特征存放至vector容器中,然后调用DBow3的字典接口即可
 * 在DBow3::Vocabulary对象的构造函数中,我们可以指定树的分支数量和深度,如默认的k=10,d=6,最大容纳1000,000个单词;
 * 对于ORB图像特征,默认参数依然是每幅图片500个特征点
 * ************************************************/


int main( int argc, char** argv )
{
    chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
    /*****************************************01.读取图片 //read the image*******************************************/
    cout<<"reading images... "<<endl;
    vector<Mat> images;
/**
//    //方式1. date中十张图片
//    for ( int i=0; i<10; i++ )
//    {
//        // string path = "./data/"+to_string(i)+".png";
//        string path = "/home/cqyd/class20/wk/tmp/tmp.CantEgyAGn/utils/data/"+to_string(i)+".png";
//        cout << path << endl;
//        images.push_back( imread(path) );
//    }

//   //方式2. TUM12_desk数据集中图片
//    for ( int i=1; i<3579; i++ )
//    {
//        // string path = "./data/"+to_string(i)+".png";
//
//        string path = "/home/cqyd/class20/dataset/DBow/tum12_desk3578/"+to_string(i)+".png";
//        //string path = "/home/cqyd/class20/dataset/DBow/tum_fr2/"+to_string(i)+".png";
//
//        cout << path << endl;
//        images.push_back( imread(path) );
//    }
*/
    //方式3. TUM数据集中图片
    // for ( int i=1; i<64054; i++ )
    for ( int i=1; i<64; i++ )
    {
        string path = "/home/cqyd/class20/dataset/DBow_wk/myvoc/"+to_string(i)+".png";
        cout << path << endl;
        images.push_back( imread(path) );
    }

    /*****************************************02.提取图像特征 // detect ORB/BEBLID features****************************/
    //cout<<"detecting ORB features ... "<<endl;
    cout<<"detecting BEBLID features ... \n"<<endl;
    Ptr< Feature2D > detector = ORB::create();
    vector<Mat> descriptors;
    for ( Mat& image:images )
    {
        vector<KeyPoint> keypoints;
        detector->detect(image, keypoints);    //beblid1
        Mat descriptor;
        auto descriptor_beblid = BEBLID::create(256, 0.75);    //beblid2
        descriptor_beblid->compute(image, keypoints, descriptor);   //beblid3
        //detector->detectAndCompute( image, Mat(), keypoints, descriptor );  //ORB描述子
        descriptors.push_back( descriptor );
    }

    /*****************************************03.创建视觉单词并保存生成词袋 // create vocabulary**************************/
    cout<<"creating vocabulary ... "<<endl;
    DBoW3::Vocabulary vocab;
    vocab.create( descriptors );    //descriptors是图像的特征点集合
    cout<<"vocabulary info: "<<vocab<<endl;
    /**
    //vocab.save("./Vocabulary/myvoc_beblid_tumfr2_19252.yml.gz" );   //生成离线词典
    //vocab.saveToTextFile("./Vocabulary/myvoc_beblid_tumfr2_19252.txt" );
    //vocab.saveToBinaryFile("./Vocabulary/myvoc_beblid_tumfr2_19252.bin");
        //2022.02.21
    vocab.saveToTextFile("./Vocabulary/myvoc_orb_tum2all_19252.txt" );
    vocab.saveToTextFile("./Vocabulary/myvoc_orb_tum12desk_3578.txt" );
     */
    vocab.saveToTextFile("./Vocabulary/myvoc_beblid_all_075_test.txt" );
    vocab.saveToBinaryFile("./Vocabulary/myvoc_beblid_all_075_test.bin");
    cout<<"save File done"<<endl;

    chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
    //统计词袋训练总时间
    double tVocTarn = chrono::duration_cast< chrono::duration<double> >(t2 - t1).count();
    //cout << "Vocabulary training times: " << tVocTarn << " seconds." << endl;
    cout << "Vocabulary training times: " << tVocTarn/60 << " minutes.\n" << endl;
    return 0;
}
